#include "solveangle.hpp"
#include <sys/time.h>

extern use_kalman_filter KF;

double dis(Point p1, Point p2){  //求两点距离
    double dis;
    dis = sqrt(pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2));
    return dis;
}

void sort_point(Point2f p[]){      //灯条点位排序
    Point2f tmp;
    int j = 2;
    while(j--){                                 //保证0，1索引值为灯条最上，2，3为最下
        for(int i = 0; i < 4 - 1; i++){
            if(p[i].y > p[i+1].y){
                tmp = p[i];
                p[i] = p[i+1];
                p[i+1] = tmp;                   //左上0，右上1
            }                                   //左下2，右下3
        }
    };

    if(p[0].x > p[1].x){            //上点位左右顺序排序
        tmp = p[0];
        p[0] = p[1];
        p[1] = tmp;
    }

    if(p[2].x > p[3].x){            //下点位左右顺序排序
        tmp = p[2];
        p[2] = p[3];
        p[3] = tmp;
    }
}

float get_angle(Point2f p[]){       //点位在进行sort_point之后为有效点位,获取矩形斜率
    float angle;
    Point2f p1((p[0].x + p[1].x)/2, (p[0].y + p[1].y)/2);
    Point2f p2((p[2].x + p[3].x)/2, (p[2].y + p[3].y)/2);
    float x = p1.x - p2.x;
    float y = -(p1.y - p2.y);
    if(x == 0)
        angle = 90;
    else if(x > 0){
        angle = atan(y/x) *180 / M_PI;
    }
    else{
        angle = 90 + atan(-x/y) *180 / M_PI;
    }
    return angle;
}

AngleSolve::AngleSolve(Mat image){
    Mat img;
    img = Img_pre_dis(image);
    Img_getb(img);
}

Mat AngleSolve::Img_pre_dis(Mat image){
    resize(image, image, Size(640, 480));
    // imshow("Display Image", image);

    image.copyTo(img);      //深拷贝（缺陷 较为消耗算力）

    int color;              //颜色模式判断
    if(color_mode == true)
        color = 0;          //蓝色模式
    else
        color = 2;          //红色模式      !!!阈值与蓝色有差别，possibly需要重新调参
    for(int i = 0; i < image.cols; i++){             //消除白色影响（非线性滤波）
        for(int j = 0; j < image.rows; j++)
        {
            int b = image.at<Vec3b>(j,i)[0];
            int g = image.at<Vec3b>(j,i)[1];
            int r = image.at<Vec3b>(j,i)[2];
            if(b > 150 && g > 150 && r > 150)
            {
                image.at<Vec3b>(j,i)[0] = 0;
                image.at<Vec3b>(j,i)[1] = 0;
                image.at<Vec3b>(j,i)[2] = 0;
            }
        }
    }

    Mat result1;
    vector<Mat> channels;
    split(image, channels);     //通道分离

    Mat B_R = channels.at(color);  //提取颜色通道

    Mat th_b;               //二值化处理
    threshold(B_R, th_b, 210, 255, THRESH_BINARY);     //阈值给定考虑：180，210阈值在后续两种滤波之下会消除最小灯条轮廓
    // imshow("thred", th_b);

    Mat resultG;
    medianBlur(th_b, th_b, 3);                 //中值滤波

    GaussianBlur(th_b, resultG, Size(3,5), 3);  //高斯低通滤波
    imshow("filter", resultG);

    // use image dilating
    Mat struc1 = getStructuringElement(0, Size(7,7));       //matrix kernel
    Mat struc2 = (Mat_<unsigned char>(9,3)<<0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0);    //user-defined kernel

    dilate(resultG, resultG, struc1);       //image dilating 1
    dilate(resultG, resultG, struc2);       //image dilating 2  user-defined
    imshow("dilate", resultG);      // 2 disposes image test

    Canny(resultG, resultG, 100, 200, 5);       //Canny算法
    imshow("edge", resultG);

    return resultG;
}

void AngleSolve::Img_getb(Mat resultG){
    vector<vector<Point>> contours;
    findContours(resultG, contours, 0, 2);  //轮廓点检测

    for(int i = contours.size() - 1; i >= 0; i--){      //筛选单个符合几何条件矩形点位(对单个矩形限制：长宽比、角度)
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];
        rrect.points(points);
        Point2f cpt = rrect.center;

        if(((dis(points[0], points[1]) / dis(points[1], points[2])) > 2.0 || (dis(points[1], points[2]) / dis(points[1], points[0])) > 2.0 )    //长宽比条件限制
        ){
            i -= 1;
            continue;
        }

        if(i >= 0){                     //pop出不符合条件点位
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }
    vector<RotatedRect> Lrect;     //定义灯条中心点vector
    for(int i = contours.size() - 1; i >= 0; i--){      //筛选灯条相关矩形点位(对多个矩形进行相关比较)
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];

        rrect.points(points);
        sort_point(points);     //sort the light points

        Point2f cpt = rrect.center;
        bool judge = false;  //条件判断标志位设立

        for(int j = contours.size() -1; j >= 0; j--){    //再次遍历contours，对coutours[i]进行相关性比较
            RotatedRect nrect = minAreaRect(contours[j]);
            Point2f pointn[4];

            nrect.points(pointn);
            sort_point(pointn);

            Point2f cpn = nrect.center;

            if(i == j)      //排除自身的相关性
                continue;

            if(
               (rrect.size.area() / nrect.size.area() < 2.0 || nrect.size.area() / rrect.size.area() < 2.0) //size limit
             &&(2.5 < dis(points[0],points[2]) / dis(points[0],points[1]))   //shape limit  (width and length)
             &&(dis(cpt, cpn) < dis(points[0],points[2]) * 4)        //location limit
             &&(abs(cpt.y - cpn.y) < dis(points[0],points[2]) / 2)

           // 0.9 < (get_slope(pointn) / get_slope(points)) < 1.1     //angle limit
             &&(abs(get_angle(pointn) - get_angle(points)) < 10)
            //  &&((get_angle(pointn) / get_angle(points)) < 1.66)

             &&(abs(points[0].x - points[1].x) > abs(points[0].y - points[1].y))     //special limit (without consider special condition: special angle matrix length < width)
            ){    //条件判断
                judge = true;      //条件满足标志位赋值
                Lrect.push_back(rrect);    //中心点位存入
                circle(img, points[0], 3, Scalar(0, 255, 0), -1, 8, 0); // 绘制圆心
                circle(img, points[2], 3, Scalar(0, 0, 255), -1, 8, 0); // 绘制圆心
                // cout << get_angle(points) << " " << get_angle(pointn) << " " << (get_angle(pointn) / get_angle(points)) << endl;
                break;
            }
        }

        if(judge == true)     //满足条件判断
            continue;

        if(i >= 0){         //移除不满足条件的矩形
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }

    vector<vector<Point2f>> B_points;   //定义装甲板点位矩形变量

    for(int i = 0; i < Lrect.size(); i++){      //装甲板矩形获取
        int L1; 

        if(Lrect[i].size.width >= Lrect[i].size.height)     //取得最长边判断1
            L1 = Lrect[i].size.width;
        else
            L1 = Lrect[i].size.height;

        for(int j = 0; j < Lrect.size(); j++){
            if(i == j)
                continue;

            if(Lrect[i].size.area() / Lrect[j].size.area() < 2.0 || Lrect[j].size.area() / Lrect[i].size.area() < 2.0
                // abs(Lrect[i].angle - Lrect[j].angle) < 40 
            ){  //限制条件判断
                Point2f cen((Lrect[i].center.x + Lrect[j].center.x)/2, (Lrect[i].center.y + Lrect[j].center.y)/2);  //取理论上亮灯条中心点

                int L2;
                if(Lrect[j].size.width >= Lrect[j].size.height)     //取得最长边判断2
                    L2 = Lrect[j].size.width;
                else
                    L2 = Lrect[j].size.height;

                vector<Point2f> tmp_p;
                tmp_p.push_back(cen);
                tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y + L1 / 2));    //LIght 1
                tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y - L1 / 2));    //Light 1 finish
                tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y - L2 / 2));    //LIght 2
                tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y + L2 / 2));    //Light 2 finished
                
                if(dis(tmp_p[1],tmp_p[2]) < dis(tmp_p[2],tmp_p[3])
                &&(dis(tmp_p[2],tmp_p[3]) / dis(tmp_p[1],tmp_p[2])) < 4.2)
                    B_points.push_back(tmp_p);
            }
        }
    }

    struct timeval tv;
    gettimeofday(&tv,NULL);
    double t = tv.tv_sec * 1000 + tv.tv_usec / 1000; // 获取当前系统时间戳

    if(!B_points.empty()){
        vector<Point2f> img_points{     //定义像素图像点位
            B_points[0][1],
            B_points[0][2],
            B_points[0][3],
            B_points[0][4]
        };

        //卡尔曼滤波测试效果                                                            **********
        // KF.stateNum = 4;
        // KF.measureNum = 2;
        // KF.init();                   卡尔曼滤波器初始化，一次性运行代码，该部分迁移到main.cpp
        KF.set_Q(0.0001);
        KF.set_R(0.01);
        // KF.set_P(1);
        // KF.x_state = (Mat_<float>(4, 1) << 100, 100, 0, 0);      该部分同上
    
        //预测阶段
        KF.get_F((Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1));
        KF.z = (Mat_<float>(2, 1) << B_points[0][0].x, B_points[0][0].y);     						//测量值
        KF.correct();

        Point2f up(KF.x_state.at<float>(0), KF.x_state.at<float>(1));
        circle(img, up, 3, Scalar(255, 0, 0), -1, 8, 0); // 绘制圆心
        circle(img, up, 6, Scalar(255, 255, 0), 3, 8, 0); //绘制空心圆                  //***********

        for(int j = 0; j < 4; j++)          //框出最小矩形
            {
                if(j == 3)      //首尾相接
                {
                    line(img, img_points[j], img_points[0], Scalar(255,0,255), 2, 8, 0);
                    break;
                }
                line(img, img_points[j], img_points[j+1], Scalar(255,0,255), 2, 8, 0);    //接出三边
            }
            circle(img, B_points[0][0], 2, Scalar(255,0,255), 2, 8, 0);  //中心点标记
            // circle(img, B_points[0][1], 2, Scalar(0,255,0), 2, 8, 0);  //wait for test
            // circle(img, B_points[0][2], 2, Scalar(0,255,0), 2, 8, 0);  //test mark
            AngleRes(img_points);
        }
        else
        {
            m_yaw = 0;
            m_pitch = 0;
        }
}

//不添加预测的yaw,pitch解算
void AngleSolve::AngleRes(vector<Point2f> img_points){     
    Eigen::Vector3d m_pc = pnp_get_pc(img_points);

    Eigen::Matrix<double, 3, 3> r_camera_ptz = Eigen::Matrix<double, 3, 3>(r_data);     //转换为Eigen::Matrix格式
    Eigen::Vector3d t_camera_ptz = Eigen::Vector3d(t_data);     //同上

    m_pc = r_camera_ptz * m_pc - t_camera_ptz; // 相机坐标系 转换为 云台坐标系

    double mc_yaw = std::atan2(m_pc(1,0), m_pc(0,0));    // yaw的测量值，单位弧度       *******
    double length = sqrt(m_pc(0, 0) * m_pc(0, 0) + m_pc(1, 0) * m_pc(1, 0));        //XY平面上的旋转角度分量
    Eigen::Vector3d c_pw{length * cos(mc_yaw), length * sin(mc_yaw), m_pc(2, 0)};   //反解位置(世界坐标系)          取得从云台坐标系反解的向量(个人认为取得云台目标yaw,pitch的关键一步)

    double distance_xy = c_pw.topRows<2>().norm();      //该向量在XY平面的投影到原点的距离
    double p_pitch = std::atan2(c_pw(2, 0), distance_xy);       // pitch的测量值，单位估计也是弧度（方向相反，反解)      *******

    float tmp_yaw = atan(c_pw(0, 0) / c_pw(2, 0));      //量纲为弧度
    float tmp_pitch = -atan(c_pw(1, 0) / c_pw(2, 0));
    if(abs(tmp_yaw) < 0.02)
        m_yaw = 0;
    else
    m_yaw = tmp_yaw;
    if(abs(tmp_pitch) < 0.02)
        m_pitch = 0;
    else
    m_pitch = tmp_pitch;
}

//取得Eigen形式tvec
Eigen::Vector3d AngleSolve::pnp_get_pc(vector<Point2f> img_points){
    Mat rvec, tvec;
    solvePnP(obj_points_simple, img_points, camera_matrix, distCoeffs, rvec, tvec);
    tvec.convertTo(tvec,CV_64F);
    Eigen::Vector3d pc;
    cv2eigen(tvec, pc);

    return pc;
}

float AngleSolve::yaw_get(){
    return m_yaw;
}

float AngleSolve::pitch_get(){
    return m_pitch;
}

Mat AngleSolve::img_get(){
    return img;
}